package rs.dev

import scala.concurrent.{ExecutionContext, Future}

class I2cDJ(port: Int, initDegree: Double = 90.0, degreeMin: Double = 0, degreeMax: Double = 180)(using ex: ExecutionContext) {

  var degree =  initDegree
  setDegree(degree)
  var degreeVelocity = 0.0d

  def startRotate(degreeVelocity: Double): Unit = {
    this.degreeVelocity = degreeVelocity
  }

  def setDegree(degree: Double): Unit = {
    if (degreeMin < degree && degree < degreeMax) {
      this.degree = degree
      //println(s"update ${port} degree to ${degree}")
      I2cDev.setPwmRate(port, degreeToRate(degree))
    }
  }

  /**
   * 0 -- 180 degree -> 0.5ms -- 2.5ms /20ms -> 2.5 -- 12.5 rate
   */
  def degreeToRate(degree: Double): Double = 2.5 + degree /180 *10

  def stop(): Unit = {
    I2cDev.setPwmRate(port, 0)
  }


  Future {
    while(true) {
      if (Math.abs(degreeVelocity) > 0.2) {
        println(s"set degree ${degree} with ${degreeVelocity} ${Math.abs(degreeVelocity)}")
        setDegree(degree + degreeVelocity)
      }
      Thread.sleep(30)
    }
  }
}
